//
// Created by xuwj on 2021/10/29.
//

#include <iostream>

#include "opencv2/core/core.hpp"
#include "opencv2/opencv.hpp"
#include "ros/ros.h"

#include "tf_publisher.h"

const string file_path = "/home/xuwj/other_workspaces/orb_track_ws/src/VNav_Assistant/configs/fixed_frames.yaml";

int main(int argc, char **argv){
    ros::init(argc,argv,"fixed_tf_publisher");
    ros::NodeHandle nh;
    ros::Rate rate(10);

    //Load FrameID
    cv::FileStorage fSettings(file_path,CV_STORAGE_READ);
    string robot_base_frame = fSettings["RobotBaseFrame"];
    string map_frame = fSettings["MapFrame"];
    string camera_frame = fSettings["CameraFrame"];

    //Load fixed transformations between frames
    cv::Mat base2cam;
    fSettings["Trc"] >> base2cam;

    //Create tf publisher
    VNavAssistant::TFPublisher TFbase_to_cam(robot_base_frame,camera_frame);

    //Broadcast
//    while(nh.ok()) {
//        TFbase_to_cam.publishTF(base2cam);
//        rate.sleep();
//    }
    TFbase_to_cam.publishTF(base2cam,true);

}